#include "CollisionDetection.h"
#include "Log.h"
#include <cmath>
#include <assert.h>

using namespace std;

template<typename T>
T& min(T& a, T& b) {	if(a < b)	return a;	return b;	}
template<typename T>
T& max(T& a, T& b) {	if(a > b)	return a;	return b;	}


bool PointRectangleCollision(const fVec2D& pt, double angle, const fVec2D& A, const fVec2D& B, const fVec2D& C, const fVec2D& D) {
	//create the orthonormal axes for the rectangle itself
	fVec2D axis1(cos(angle), sin(angle));
	fVec2D axis2(-sin(angle), cos(angle));
	
	//Project everything onto the first axis
	double aProj = A | axis1;
	double bProj = B | axis1;
	double cProj = C | axis1;
	double dProj = D | axis1;
	double ptProj = pt | axis1;
	
	double minProj = min(min(min(aProj, bProj), cProj), dProj);
	double maxProj = max(max(max(aProj, bProj), cProj), dProj);
	
	//if the pt doesn't lie in the segment of the projection
	if(ptProj < minProj || ptProj > maxProj)
		return false;
		
	//Project everything onto the second axis
	aProj = A | axis2;
	bProj = B | axis2;
	cProj = C | axis2;
	dProj = D | axis2;
	ptProj = pt | axis2;
	
	minProj = min(min(min(aProj, bProj), cProj), dProj);
	maxProj = max(max(max(aProj, bProj), cProj), dProj);
	
	//if the pt doesn't lie in the segment of the projection
	if(ptProj < minProj || ptProj > maxProj)
		return false;

	//Neither projection yielded a non-collision, therefore we must have collided
	return true;
/*
//This is code taken from my implementation in Tekkotsu, which does fancy matrix math for you
	fmat::Column<2> rectCent;
	getCenterPoint(&(rectCent[0]), &(rectCent[1]), NULL, NULL, NULL, NULL);

	fmat::Column<2> diff = rectCent - other.centerPoint;
	if(diff.norm() < other.radius)
		return true;

	fmat::Column<2> testPt = diff /= diff.norm();
	testPt *= other.radius;
	testPt += other.centerPoint;
	
	for(unsigned int k = 0; k < 2; k++) {
		fmat::Row<2> axisT = transpose(rot.column(k));

		const fmat::Row<4> projA = points * axisT;
		const fmat::Row<1> projB = testPt * axisT;

		const float minA = findMin(projA);
		const float maxA = findMax(projA);

		if(projB[0] < minA || projB[0] > maxA)
			return false;
	}
*/
}

/*
Taken from:
	Tomas Moller of Chalmers University of Technology
	Ben Trumbore of Cornell University
*/
bool RayTriangleCollision(const fVec3D& orig, const fVec3D& dir, const lVec3D& _vert0, const lVec3D& _vert1, const lVec3D& _vert2, lVec3D &_intersection) {
	static double EPSILON = 0.01;
	
	//Have to do the calculation in floating point, integer calculation barfs
	Point3D<double> vert0 = convert(_vert0), vert1 = convert(_vert1), vert2 = convert(_vert2), intersection = convert(_intersection);
	Point3D<double> edge1, edge2, tvec, pvec, qvec;
	double det, inv_det, t, u, v;
	
	edge1 = vert1 - vert0;
	edge2 = vert2 - vert0;
	pvec = dir ^ edge2;
	det = edge1 | pvec;
	
	if(std::fabs(det) < EPSILON)	//if the ray is parallel to the plane of the triangle
		return false;
		
	inv_det = 1.0 / det;
	
	tvec = orig - vert0;
	u = (tvec | pvec) * inv_det;
	if(u < 0 || u > 1)			//if the projection of the point doesn't fall into the range of the first leg
		return false;
		
	qvec = tvec ^ edge1;
	v = (dir | qvec) * inv_det;
	if(v < 0 || u+v > 1)		//if the projection of the point doesn't fall into the range of the triangle based on the second leg
		return false;
		
	t = (edge2 | qvec) * inv_det;	//Calculate the parametric difference
	
	intersection = orig + (dir * t);	//The actual point
	_intersection = convert(intersection);
	return true;
}


